import numpy as np
from Message import MTX
import rospy
import tf
from geometry_msgs.msg import PointStamped

def Point2DTo3D(point,dis):
    u,v = point
    point1 = np.array([u*dis,v*dis,dis])
    inmtx = np.linalg.inv(MTX)
    point3D = np.matmul(inmtx ,point1)
    return point3D

class PointTransform():
    def __init__(self):
        self.listener = tf.TransformListener()
        self.rot = None
        self.trans = None
        self.get_transform("panda_link0", "camera_color_optical_frame")

    def get_transform(self, from_frame, to_frame):

        self.listener.waitForTransform(from_frame, to_frame, rospy.Time(0), rospy.Duration(3))
        (trans, rot) = self.listener.lookupTransform(from_frame, to_frame, rospy.Time(0))
        self.trans = trans
        self.rot = rot

    def transformation(self,point):
  
        rotation_matrix = tf.transformations.quaternion_matrix(self.rot)[:3, :3]

        transform_matrix = np.eye(4)
        transform_matrix[:3, :3] = rotation_matrix 
        transform_matrix[:3, 3] = self.trans

        input_point = np.concatenate([point, [1]]) 
        transformed_point = np.dot(transform_matrix, input_point) 
        
        return transformed_point[:3]
# --------------------------------------

def main_pointtransfrom():
    rospy.init_node('tf_subscriber') 
    p = PointTransform()
    print(p.transformation([0,0,1]))

if __name__ == '__main__':
    main_pointtransfrom()